#pragma once

#include "octree.h"
#include "vector.h"
#include "sphere.h"
#include "ray.h"
#include <vector>
#include <unordered_set>
#include <tuple>
#include <queue>
#include <fstream>
#include <sstream>
#include <cassert>
#include "mytimer.h"


#define THREE_D
//#define DEBUG                   //是否打印相关信息
#define TIMEOUTMSEC 100         //屏幕刷新间隔ms
#define LASERSCAN_R 2          //激光器扫描半径,也即删除半径
#define OCTREE_DEPTH 0         //八叉树深度
#define POINTSNUMS 10000000    //点的总数
#define SCANNUMS 50            //扫描插入点的数量
#define VIEWHEIGHT 50            //视角高度
#define VIEWDEPTH  100          //视野距离
//#define SCAN                    //是否开启激光扫描
//#define ROAD                //是否显示路径
//#define POINTCLOUD              //点云
#define EPSILON 1e-6

#define EPS 0.001
#define EQ(A,B) (abs((A)-(B)) <= EPS)
#define LT(A,B) ((((A)-(B)) < 0) && (abs((A)-(B)) > EPS))
#define GT(A,B) ((((A)-(B)) > 0) && (abs((A)-(B)) > EPS))
#define LTE(A,B) (((A)-(B)) < 0)
#define GTE(A,B) (((A)-(B)) > 0)

//x的y位置1
#define setbit(x,y)  (x)|=(1<<(y))
//x的y位置0
#define clrbit(x,y)  (x)&=~(1<<(y))
//x的y位取反
#define reversebit(x,y)  (x)^=(1<<(y))
//获的x的y位
#define getbit(x,y)   ((x) >> (y)&1)

struct tuple_hash
{
    std::size_t operator() (const std::tuple<Point*,OctreeNode*>& A) const {
        return size_t(std::get<0>(A));
    };
};

struct tuple_equal
{
    bool operator()(const std::tuple<Point*,OctreeNode*>& A,const std::tuple<Point*,OctreeNode*>& B) const {
        return ((std::get<0>(A) == std::get<0>(B)) ? true : false);
        //return true;
    }
};

extern const float PI;
extern int count_for_collisionpoint;


extern std::vector<double> record_theta_tan;
extern std::vector<double> record_theta_cos;
extern std::vector<double> record_theta_sin;
extern std::vector<OctreeNode*> waynode; //激光射线沿线经过的节点


extern std::vector<OctreeNode*> collisionnode;//发生碰撞的八叉树节点
extern std::vector<OctreeNode*> collisionnode_2;
extern std::vector<OctreeNode*> collisionnode_3;
extern std::vector<OctreeNode*> collisionnode_4;

extern std::unordered_set<Point*> results;    //发生碰撞的点
extern std::unordered_set<Point*> results_2;
extern std::unordered_set<Point*> results_3;
extern std::unordered_set<Point*> results_4;

extern std::vector<OctreeNode*> scannode;     //扫描到的八叉树节点
extern std::vector<OctreeNode*> rayintersectnode;
extern std::unordered_set<std::tuple<Point*,OctreeNode*>,tuple_hash,tuple_equal> scanpoint;  //光线相交的点

extern std::unordered_set<Point*> results_for_scanpoint;

extern std::unordered_set<uint64_t> DATUM_FRONT;
extern std::unordered_set<uint64_t> DATUM_BACK;
extern std::unordered_set<uint64_t> DATUM_BEYOND;
extern std::unordered_set<uint64_t> DATUM_BOTTOM;
extern std::unordered_set<uint64_t> DATUM_LEFT;
extern std::unordered_set<uint64_t> DATUM_RIGHT;


//读取csv文件点云数据
std::vector<std::vector<double>> read_gt_file(std::string filename);

//提供碰撞检测的函数，具体用法见function.cpp 提供三种算法：包围球、AABB、OBB
bool abortTesting(const float& znodedown,const float& zrobotup,const float& znodeup,const float& zrobotdown);
bool isSeparatingPlane(const Vector& RPos, const Vector& Plane, const Cuboid& box1, const Cuboid& box2);
bool isSeparatingLine(const Cuboid&box1,const Cuboid& box2, const Vector& axis,const Vector& delta);
bool isCollisionOBB(const Cuboid& box1, const Cuboid& box2);
bool isCollisionOBB_V2(const Cuboid &a, const Cuboid &b);
bool isCollisionOBB2D(const Cuboid& box1, const Cuboid& box2);
bool isCollisionSphere(const Cuboid& octree, const Cuboid& robot);
bool isCollisionAABB(const Cuboid& octree,const Cuboid& robot);
bool isCollisionProject(Point* p, const Cuboid& robot);
void getCollisionOctreeNodeSphere(OctreeNode* node, const Cuboid& robot);
void getCollisionOctreeNodeAABB(OctreeNode* node, const Cuboid& robot);
void getCollisionOctreeNodeOBB(OctreeNode* node, const Cuboid& robot);
void getCollisionOctreeNodeOBB2D(OctreeNode* node, const Cuboid& robot);
void getCollisionOctreeNodeOBBThread(OctreeNode* node, const Cuboid& robot, std::vector<OctreeNode *> &collisionnode);
bool getCollisionPoint(const std::vector<OctreeNode*>& collisionnode, const Cuboid& robot);
bool getCollisionPointThread(const std::vector<OctreeNode*>& collisionnode, const Cuboid& robot,std::unordered_set<Point*>& results);

//提供扫描检测的函数
bool isScanAABB(const Cuboid& octree, const Cuboid& laser);
bool isScanPoint(Point* p,const Cuboid& laser);
void getScanOctreeNodeSphere(OctreeNode* node, const Cuboid& laser);
double getMinDistance(const std::vector<OctreeNode*>& scannode, const Cuboid& laser,const std::vector<Cuboid>& robotparts);
void laserScanning(OctreeNode* octree,const Cuboid& laser);
void laserScanningByNeighbor(Octree* octree,const Cuboid& laser,std::unordered_set<std::tuple<Point*,OctreeNode*>,tuple_hash,tuple_equal>& result_set,const float& start_angle,const float& end_angle);
void deleteObject(std::vector<OctreeNode*>& scannode,const Cuboid& laser);
bool deduplication(Point* point,OctreeNode* node);
void test();

//提供光线相交测试的函数
float getRayIntersectDistance(const Sphere& sphere, const Ray& light);
bool isRayIntersectAABB(const Ray& light, const Cuboid& AABB);
void getIntersectOctreeNode(OctreeNode* node,const Ray& light);
std::tuple<Point*,OctreeNode*> getIntersectPoint(const std::vector<OctreeNode*>& intersectnodes,const Ray& light);
void getTrigonometricFunction(const double& angle_resolution);
void getVirtureSurroundPoint(Octree* octree,Ray& ray,const double& laser_length,const double& angle_resolution,const float& start_angle,const float& end_angle,
                             std::unordered_set<std::tuple<Point*,OctreeNode*>,tuple_hash,tuple_equal>& result_sets);
std::tuple<Point*,OctreeNode*> getRayIntersectPoint(OctreeNode* octree,Ray& light);
uint64_t getDatum(uint64_t MortanCode);

//利用光线追踪的结果进行近邻搜索
double SqDistPointOBB(const Point& p,const Cuboid& OBB);

//将两个OBB近似为一个OBB,并且总是使用第一个OBB的主方向作为主轴，在步长较短的情况下，碰撞域的增加并不大
Cuboid buildOBB(const Cuboid& A, const Cuboid& B);

Point operator+(const Point& lhs, const Point& rhs);
Point operator+(const Point& lhs, const Vector& rhs);
Point operator+(const Vector& lhs, const Point& rhs);

Point operator-(const Point& lhs, const Point& rhs);
//点乘-1
float operator* (const Point& lhs, const Point& rhs);
//点乘-2
Point operator* (const Point& lhs, const float& rhs);
//叉乘
Point operator^ (const Point& lhs, const Point& rhs);
